#include "MyRobot.h"

Robot::Robot():
_testSpeed(0),_maxVolts(12.0), _testPos(0)
{
	_init();	
}

Robot::~Robot()
{	
	return;
}

void Robot::_init()
{	
//----------------------------------------------------------------
// Joystick Initialization
//----------------------------------------------------------------
		Joy = new Joystick(1); 
//----------------------------------------------------------------
// Jaguar Initialization
//----------------------------------------------------------------
		JagTEST = new CANJaguar(3, CANJaguar::kSpeed);
		Wait(0.01);
		JagTEST->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		JagTEST->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		Wait(0.01);
		JagTEST->ConfigFaultTime(0.7f);
		Wait(0.01);
		JagTEST->EnableControl(0.0f);
		Wait(0.01);
		JagTEST->ConfigMaxOutputVoltage(_maxVolts);
		Wait(0.01);
		JagTEST->SetPID(30.0, 0.0, 0.0);
		Wait(0.01);
		JagTEST->ConfigEncoderCodesPerRev(512);	
		JagTEST->EnableControl(50.0);
		JagTEST->Disable();
		
		_testPos = JagTEST->GetPosition();
}
void Robot::RobotInit()
{
	this->GetWatchdog().SetEnabled(false);
	this->GetWatchdog().SetExpiration(1.0);

	return;
}
//----------------------------------------------------------------
// Autonomous Init
//----------------------------------------------------------------
void Robot::AutonomousInit()
{
	this->GetWatchdog().SetEnabled(false);
	this->GetWatchdog().Feed();
	this->GetWatchdog().SetExpiration(0.5);
	
	this->SetPeriod(0.25);
	
	return;
}
//----------------------------------------------------------------
// Autonomous Runtime loop
//----------------------------------------------------------------
void Robot::AutonomousContinuous()
{	
	this->GetWatchdog().Feed();	
	return;
}
void Robot::AutonomousPeriodic()
{
	this->GetWatchdog().Feed();
	return;
}
void Robot::DisabledContinuous()
{
	return;
}		
void Robot::DisabledInit()
{
	this->GetWatchdog().SetEnabled(true);
	this->GetWatchdog().Feed();
	this->GetWatchdog().SetExpiration(1.0);

	this->SetPeriod(0.05);
	
	return;
}
void Robot::DisabledPeriodic()
{
	this->GetWatchdog().Feed();
	return;
}

void Robot::TeleopContinuous() 
{
	this->GetWatchdog().Feed();
	
	
	return;
}
void Robot::clampSpeeds()
{
	this->GetWatchdog().Feed();

	clamp(_testSpeed);
		
	return;
}
void Robot::clamp(float & val)
{
	if(val < -12.0f) val = -12.0f;
	if(val > 12.0f)	val = 12.0f;
}
void Robot::TeleopInit()
{
	this->GetWatchdog().SetExpiration(1.0);
	this->GetWatchdog().SetEnabled(true);
	this->GetWatchdog().Feed();
	
	this->SetPeriod(0.1);
	
	return;
}
void Robot::TeleopPeriodic() 
{
	this->GetWatchdog().Feed();

	if(Joy->GetRawButton(3)) JagTEST->Disable();
	if(Joy->GetRawButton(2))
	{
		JagTEST->ChangeControlMode(CANJaguar::kSpeed);
		JagTEST->SetSpeedReference(CANJaguar::kSpeedRef_Encoder);
		JagTEST->EnableControl(0.0);
	}
	if(Joy->GetRawButton(8))
	{
		JagTEST->ChangeControlMode(CANJaguar::kPosition);
		JagTEST->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
		JagTEST->EnableControl(0.0);
	}
	
	else if(Joy->GetRawButton(5)) JagTEST->Set(5.0);
	else if(Joy->GetRawButton(4)) JagTEST->Set(-5.0);
	else if(Joy->GetRawButton(1)) JagTEST->Set(0.0);
	else if(Joy->GetRawButton(9))
	{
		JagTEST->ChangeControlMode(CANJaguar::kVoltage);
		JagTEST->EnableControl(0.0);
	}

	
	return;
}


START_ROBOT_CLASS(Robot);
